#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <string>
#include <fstream>
#include <sstream>
#include <vector>
#include <boost/filesystem.hpp>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include "auto_navigation/utils/kitti_data_player.hpp"

namespace fs = boost::filesystem;

namespace auto_navigation {

KittiDataPlayer::KittiDataPlayer(ros::NodeHandle& nh, ros::NodeHandle& private_nh)
    : nh_(nh), private_nh_(private_nh), play_rate_(10.0), current_frame_(0) {
    
    // Get parameters
    private_nh_.param<std::string>("kitti_data_path", kitti_data_path_, "");
    private_nh_.param<double>("play_rate", play_rate_, 10.0);
    private_nh_.param<bool>("loop", loop_, true);
    private_nh_.param<std::string>("frame_id", frame_id_, "velodyne");
    
    // Initialize publishers
    points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/points_raw", 10);
    
    // Load file list
    if (!loadFileList()) {
        ROS_ERROR("Failed to load file list from %s", kitti_data_path_.c_str());
        return;
    }
    
    // Start timer
    play_timer_ = nh_.createTimer(ros::Duration(1.0 / play_rate_), 
                                 &KittiDataPlayer::timerCallback, this);
    
    // Publish static transform
    publishStaticTransforms();
    
    ROS_INFO("KITTI Data Player initialized with %zu files", file_list_.size());
}

KittiDataPlayer::~KittiDataPlayer() {}

bool KittiDataPlayer::loadFileList() {
    fs::path data_path(kitti_data_path_);
    if (!fs::exists(data_path) || !fs::is_directory(data_path)) {
        ROS_ERROR("Invalid KITTI data path: %s", kitti_data_path_.c_str());
        return false;
    }
    
    // Find velodyne_points/data directory
    fs::path velodyne_data_path = data_path / "velodyne_points" / "data";
    if (!fs::exists(velodyne_data_path) || !fs::is_directory(velodyne_data_path)) {
        ROS_ERROR("Velodyne data directory not found: %s", velodyne_data_path.string().c_str());
        return false;
    }
    
    // Get all .bin files
    for (fs::directory_iterator it(velodyne_data_path); it != fs::directory_iterator(); ++it) {
        if (fs::is_regular_file(it->path()) && it->path().extension() == ".bin") {
            file_list_.push_back(it->path().string());
        }
    }
    
    // Sort files by name
    std::sort(file_list_.begin(), file_list_.end());
    
    return !file_list_.empty();
}

void KittiDataPlayer::timerCallback(const ros::TimerEvent& event) {
    if (file_list_.empty()) {
        return;
    }
    
    if (current_frame_ >= file_list_.size()) {
        if (loop_) {
            current_frame_ = 0;
            ROS_INFO("Looping back to first frame");
        } else {
            ROS_INFO("Reached end of dataset");
            return;
        }
    }
    
    // Read and publish current frame
    publishPointCloud(file_list_[current_frame_]);
    current_frame_++;
}

void KittiDataPlayer::publishPointCloud(const std::string& file_path) {
    // Open file
    std::ifstream file(file_path.c_str(), std::ios::binary);
    if (!file.is_open()) {
        ROS_ERROR("Failed to open file: %s", file_path.c_str());
        return;
    }
    
    // Get file size
    file.seekg(0, std::ios::end);
    const size_t file_size = file.tellg();
    file.seekg(0, std::ios::beg);
    
    // KITTI point cloud format: x, y, z, intensity (4 floats per point)
    const size_t num_points = file_size / (4 * sizeof(float));
    
    // Create point cloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    cloud->reserve(num_points);
    
    // Read all points
    for (size_t i = 0; i < num_points; ++i) {
        float data[4];
        file.read(reinterpret_cast<char*>(data), 4 * sizeof(float));
        
        pcl::PointXYZI point;
        point.x = data[0];
        point.y = data[1];
        point.z = data[2];
        point.intensity = data[3];
        
        cloud->push_back(point);
    }
    
    file.close();
    
    // Convert to ROS message
    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(*cloud, cloud_msg);
    cloud_msg.header.frame_id = frame_id_;
    cloud_msg.header.stamp = ros::Time::now();
    
    // Publish
    points_pub_.publish(cloud_msg);
    
    ROS_INFO_THROTTLE(1.0, "Published point cloud from %s with %zu points", 
                     fs::path(file_path).filename().string().c_str(), cloud->size());
}

void KittiDataPlayer::publishStaticTransforms() {
    // Publish static transform from base_link to velodyne
    geometry_msgs::TransformStamped transform;
    transform.header.stamp = ros::Time::now();
    transform.header.frame_id = "base_link";
    transform.child_frame_id = frame_id_;
    transform.transform.translation.x = 0.0;
    transform.transform.translation.y = 0.0;
    transform.transform.translation.z = 1.73; // Typical height of velodyne on KITTI car
    transform.transform.rotation.w = 1.0;
    transform.transform.rotation.x = 0.0;
    transform.transform.rotation.y = 0.0;
    transform.transform.rotation.z = 0.0;
    
    static_broadcaster_.sendTransform(transform);
}

} // namespace auto_navigation 